clc
clear all

data1 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation/sim_leg_cali_2.csv');
data = table2array(data1);
data_q = data(:,1:5)';
data_qd = data(:,6:10)';
data_tau = data(:,11:15)';

data2 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation/real_leg_cali.csv');
data_ = table2array(data2);
data_q_ = data_(:,1:5)';
data_qd_ = data_(:,6:10)';
data_tau_ = data_(:,11:15)';


t = 0.002;%采样周期 0.002s
N_data =size(data_q,2);
data_qdd = zeros(5,N_data - 1);
data_qdd_ = zeros(5,N_data - 1);

data_qd_deri_ = zeros(5,N_data - 1);
data_qdd_deri_ = zeros(5,N_data - 2);


for i = 1:N_data -1
    data_qdd(:,i) = (data_q(:,i+1) - data_q(:,i))/t; 
    data_qdd_(:,i) = (data_qd_(:,i+1) - data_qd_(:,i))/t; 

end


for i = 1:N_data -1
    data_qdd(:,i) = (data_qd(:,i+1) - data_qd(:,i))/t; 
    data_qdd_(:,i) = (data_qd_(:,i+1) - data_qd_(:,i))/t; 

end


%%计算差分得到的速度和加速度
for i = 1:N_data -1
    data_qd_deri_(:,i) = (data_q_(:,i+1) - data_q_(:,i))/t; 
end
for i = 1:N_data -2
    data_qdd_deri_(:,i) = (data_qd_deri_(:,i+1) - data_qd_deri_(:,i))/t; 
end


% for i = 1:N_data -2
%     data_qdd_(:,i) = (data_qd_(:,i+1) - data_qd_(:,i))/t; 
% 
% end

% 一阶滤波公式y(n) = alpha*x(n) +(1-alpha)*y(n-1)

alpha = 0.05;
data_qdd_filted_ = data_qdd_;
data_qd_filted_ = data_qd_;
data_q_filted_ = data_q_;
data_tau_filted_ =data_tau_;
for i = 1:N_data -2
    data_qdd_filted_(:,i+1) = alpha*data_qdd_filted_(:,i+1) + (1-alpha)*data_qdd_filted_(:,i);
    data_qd_filted_(:,i+1) = alpha*data_qd_filted_(:,i+1) + (1-alpha)*data_qd_filted_(:,i);
    data_q_filted_(:,i+1) = alpha*data_q_filted_(:,i+1) + (1-alpha)*data_q_filted_(:,i);
    data_tau_filted_(:,i+1) = alpha*data_tau_filted_(:,i+1) + (1-alpha)*data_tau_filted_(:,i);

end


figure(6)
plot(data_tau_(3,:)','g')
hold on;
plot(data_tau_filted_(3,:)','b')
hold on;
plot(data_tau(3,:)','r')
hold on

figure(7)
plot(data_qd_(3,:)','g')
hold on
plot(data_qd_filted_(3,:)','b')
hold on
plot(data_qd(3,:)','r')
hold on


figure(8)
plot(data_qdd_(3,:)','g')
hold on
plot(data_qdd_filted_(3,:)','b')
hold on
plot(data_qdd(3,:)','r')
hold on


figure(9)

plot(data_qd_(3,:)','b')
hold on
plot(data_qd_deri_(3,:)','g')
hold on

figure(10)

plot(data_qdd_(3,:)','b')
hold on
plot(data_qdd_deri_(3,:)','g')
hold on